#!/usr/bin/env python
import rospy
from sensor_msgs.msg import JointState
import threading
import time

class RotateWheelNode:
    def __init__(self):
        rospy.init_node('rotate_fishbot_wheel', anonymous=True)
        self.joint_states_publisher = rospy.Publisher('joint_states', JointState, queue_size=10)
        self._init_joint_states()
        self.pub_rate = rospy.Rate(30)  # 30Hz
        self.thread = threading.Thread(target=self._thread_pub)
        self.thread.start()

    def _init_joint_states(self):
        self.joint_speeds = [0.0, 0.0]
        self.joint_states = JointState()
        self.joint_states.header.stamp = rospy.Time.now()
        self.joint_states.header.frame_id = ""
        self.joint_states.name = ['left_wheel_joint', 'right_wheel_joint']
        # self.joint_states.name = ['laser_joint', 'right_wheel_joint']
        self.joint_states.position = [0.0, 0.0]
        self.joint_states.velocity = self.joint_speeds
        self.joint_states.effort = []

    def update_speed(self, speeds):
        self.joint_speeds = speeds

    def _thread_pub(self):
        last_update_time = time.time()
        while not rospy.is_shutdown():
            delta_time = time.time() - last_update_time
            last_update_time = time.time()
            self.joint_states.position[0] += delta_time * self.joint_states.velocity[0]
            self.joint_states.position[1] += delta_time * self.joint_states.velocity[1]
            self.joint_states.velocity = self.joint_speeds
            self.joint_states.header.stamp = rospy.Time.now()
            self.joint_states_publisher.publish(self.joint_states)
            self.pub_rate.sleep()

def main():
    node = RotateWheelNode()
    node.update_speed([15.0, -15.0])
    rospy.spin()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass